#include <lcm/lcm.h>
#include <bot_core/bot_core.h>
#include <lcmtypes/quad_lcmtypes.h>
#include <quad_controller/controller_interface.h>

//More headers of the libraries we'll need
#include <assert.h>
#include <signal.h>
#include <unistd.h>

static lcm_t * lcm = NULL;
static carmen3d_log_annotate_msg_t annotate_msg;

void shutdown_module(int unused __attribute__((unused))) {
  printf("shutting down!\n");
  exit(1);
}

int main(int argc, char **argv) {
  lcm = bot_lcm_get_global(NULL);
  //annotate_msg.group = "controller";

  srand((unsigned) bot_timestamp_now());

  quad_waypoint_t waypt_message;
  bool send_thru_planner = 0;

  signal(SIGINT, shutdown_module);

  double repeat = -1;
  while (1) {
    //default values
    carmen3d_controller_set_noop(&waypt_message);
    int validWpt = 0;

    // "land"
    if (argc == 2 && strcmp(argv[1], "land") == 0) {
      printf("sending LAND \n ");
      carmen3d_controller_publish_land_waypoint(lcm, bot_timestamp_now());
      return 0;
    }
    // "takeoff"
    else if (argc == 2 && strcmp(argv[1], "takeoff") == 0) {
      printf("sending TAKEOFF \n ");
      carmen3d_controller_publish_takeoff_waypoint(lcm, bot_timestamp_now());
      annotate_msg.annotation = (char *) "takeoff";
      annotate_msg.utime = bot_timestamp_now();
      carmen3d_log_annotate_msg_t_publish(lcm, LOG_ANNOTATE_CHANNEL,
                                          &annotate_msg);
      return 0;
    }
    // "stop"
    else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
      printf("sending USER_STOP \n");
      carmen3d_controller_publish_user_stop_waypoint(lcm, bot_timestamp_now());
      return 0;
    }
    // "origin"
    else if (argc == 2 && strcmp(argv[1], "origin") == 0) {
      printf("sending to ORIGIN \n");
      waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
      waypt_message.xyzt[0] = 0;
      waypt_message.xyzt[1] = 0;
      waypt_message.xyzt[2] = 0.5;
      waypt_message.xyzt[3] = 0;
      waypt_message.xyzt_dot[0] = 1;
      waypt_message.xyzt_dot[1] = 1;
      waypt_message.xyzt_dot[2] = 1;
      waypt_message.xyzt_dot[3] = 0.5;
      validWpt = 1;
    }
    // "random": random <optional_arg repeat>
    // send to random point within vicon bounds
    else if (argc >= 2 && argc <= 3 && strcmp(argv[1], "random") == 0) {
      waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
      waypt_message.xyzt[0] = bot_randf_in_range(-1, 1);
      waypt_message.xyzt[1] = bot_randf_in_range(-1, 1);
      waypt_message.xyzt[2] = bot_randf_in_range(1, 1.5);
      waypt_message.xyzt[3] = bot_randf_in_range(-M_PI, M_PI);

      waypt_message.xyzt_dot[0] = bot_randf_in_range(.5, 1.3);
      waypt_message.xyzt_dot[1] = bot_randf_in_range(.5, 1.3);
      waypt_message.xyzt_dot[2] = bot_randf_in_range(.2, 1.2);
      waypt_message.xyzt_dot[3] = bot_randf_in_range(.2, .7);
      validWpt = 1;
      if (argc == 3 && atoi(argv[2]))
        repeat = atof(argv[2]);
      printf("sending random WAYPT: ");

    }
    // "random_per" <fraction_of_max_velocities>
    // actually fraction (eg 0.1) not percent
    else if (argc >= 2 && argc <= 4 && strcmp(argv[1], "random_per") == 0) {
      validWpt = 1;
      double vel_percent;
      if (argc >= 3 && sscanf(argv[2], "%lf", &vel_percent) != 1) {
        printf("Error: Couldn't read the z value\n");
        validWpt = 0;
      } else {
        printf("Using Velocity Percent: %f\n", vel_percent);
        waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
        waypt_message.xyzt[0] = bot_randf_in_range(-1, 1);
        waypt_message.xyzt[1] = bot_randf_in_range(-1, .1);
        waypt_message.xyzt[2] = bot_randf_in_range(.3, 1.4);
        waypt_message.xyzt[3] = bot_randf_in_range(-M_PI, M_PI);

        waypt_message.xyzt_dot[0] = vel_percent * 2;
        waypt_message.xyzt_dot[1] = vel_percent * 2;
        waypt_message.xyzt_dot[2] = vel_percent * 1.5;
        waypt_message.xyzt_dot[3] = vel_percent * 0.7;
        validWpt = 1;
        //if (argc == 3 && atoi(argv[2]))
        //repeat = 1;
        printf("sending random WAYPT: ");
      }

    }
    // "height" <height (m)>
    else if (argc >= 2 && argc <= 4 && strcmp(argv[1], "height") == 0) {
      validWpt = 1;
      waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
      if (argc >= 3 && sscanf(argv[2], "%lf", &waypt_message.xyzt[2]) != 1) {
        printf("Error: Couldn't read the z value\n");
        validWpt = 0;
      }
      if (argc >= 4
          && sscanf(argv[3], "%lf", &waypt_message.xyzt_dot[2]) != 1) {
        printf("Error: Couldn't read dz value\n");
        validWpt = 0;
      }

      //sanity checks
      if (waypt_message.xyzt[2] <= .1 || waypt_message.xyzt[2] > 3.5) {
        printf("Error: z must be between 0 and 3.5m\n");
        validWpt = 0;
      }
      if ((waypt_message.xyzt_dot[2] <= 0 || waypt_message.xyzt_dot[2] > 1)
          && waypt_message.xyzt_dot[2] != QUAD_WAYPOINT_T_USE_DEFAULT) {
        printf("Error: vz must be greateer than 0 and less than 1 m/s!\n");
        validWpt = 0;
      }
    }
    // "yaw" <absolute yaw angle (deg)>
    else if (argc == 3 && strcmp(argv[1], "yaw") == 0) {
      validWpt = 1;
      waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
      if (sscanf(argv[2], "%lf", &waypt_message.xyzt[3]) != 1) {
        printf("ERROR: couldn't read the yaw value\n");
        validWpt = 0;
      }
      waypt_message.xyzt[3] = bot_to_radians(waypt_message.xyzt[3]);

      //sanity check
      if (waypt_message.xyzt[3] < -M_PI || waypt_message.xyzt[3] > M_PI) {
        printf("Error: yaw value must be betw -180 & 180\n");
        validWpt = 0;
      }
      if (validWpt)
        printf("sending yaw: ");
    }
    // "step" - CONTROL STEP COMMAND
    // step <pitch> <roll> <yaw> <thrust> <duration>
    // if value=-1, keeps previous control
    //    else if (argc == 7 && strcmp(argv[1], "step") == 0) {
    //      validWpt = 1;
    //      waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
    //      carmen3d_control_step_msg_t step_msg;
    //      step_msg.utime = bot_timestamp_now();
    //      // read args
    //      if (sscanf(argv[2], "%lf", &step_msg.pitch) != 1) {
    //        printf("ERROR: couldn't read the pitch step value\n");
    //        validWpt = 0;
    //      }
    //      if (sscanf(argv[3], "%lf", &step_msg.roll) != 1) {
    //        printf("ERROR: couldn't read the roll step value\n");
    //        validWpt = 0;
    //      }
    //      if (sscanf(argv[4], "%lf", &step_msg.yaw) != 1) {
    //        printf("ERROR: couldn't read the yaw step value\n");
    //        validWpt = 0;
    //      }
    //      if (sscanf(argv[5], "%lf", &step_msg.thrust) != 1) {
    //        printf("ERROR: couldn't read the thrust step value\n");
    //        validWpt = 0;
    //      }
    //      if (sscanf(argv[6], "%lf", &step_msg.duration) != 1) {
    //        printf("ERROR: couldn't read the control step duration\n");
    //        validWpt = 0;
    //      }
    //      // check bounds
    //      if (step_msg.pitch != -1 && (step_msg.pitch < -1.5 || step_msg.pitch > 1.5)) {
    //        printf("Error: pitch value must be -0.6:0.6\n");
    //        validWpt = 0;
    //      }
    //      if (step_msg.roll != -1 && (step_msg.roll < -1.5 || step_msg.roll > 1.5)) {
    //        printf("Error: roll value must be -0.6:0.6\n");
    //        validWpt = 0;
    //      }
    //      if (step_msg.yaw != -1 && (step_msg.yaw < -1.5 || step_msg.yaw > 1.5)) {
    //        printf("Error: yaw value must be -0.4:0.4\n");
    //        validWpt = 0;
    //      }
    //      if (step_msg.thrust != -1 && (step_msg.thrust < 0.4 || step_msg.thrust > 1.6)) {
    //        printf("Error: thrust value must be 0.5:0.75\n");
    //        validWpt = 0;
    //      }
    //      if (step_msg.duration < 0 || step_msg.duration > 2.0) {
    //        printf("Error: step duration must be 0:2 seconds\n");
    //        validWpt = 0;
    //      }
    //      // send message
    //      if (validWpt) {
    //        fprintf(stderr, "Sending STEP: p:%f r:%f y:%f t:%f duration:%f\n", step_msg.pitch, step_msg.roll, step_msg.yaw,
    //            step_msg.thrust, step_msg.duration);
    //        carmen3d_control_step_msg_t_publish(lcm, "CONTROL_STEP_CHANNEL", &step_msg);
    //        return 0;
    //      }
    //    }
    //    // pitch sweep
    //    else if (argc == 3 && strcmp(argv[1], "pitch_sweep") == 0) {
    //      validWpt = 1;
    //      //waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
    //      waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_RELATIVE;
    //      double p_mag;
    //      if (sscanf(argv[2], "%lf", &p_mag) != 1) {
    //        printf("ERROR: couldn't read the pitch sweep magnitude\n");
    //        validWpt = 0;
    //      }
    //      waypt_message.xyzt[0] = 2;
    //      waypt_message.xyzt[1] = 0;
    //      waypt_message.xyzt[2] = 1;
    //      waypt_message.xyzt[3] = 0;
    //      waypt_message.xyzt_dot[0] = 2;
    //      waypt_message.xyzt_dot[1] = 0;
    //      waypt_message.xyzt_dot[2] = 0;
    //      waypt_message.xyzt_dot[3] = 0;
    //
    //      if (validWpt)
    //        printf("sending pitch sweep: ");
    //    }
    // full waypoint,velocity command
    // waypoint_tool <x> <y> <z> <yaw> <x_dot> <y_dot> <z_dot> <yaw_dot>
    // "yaw" <absolute yaw angle (deg)>

    // waypoint_tool planner <x> <y> <z> <yaw>
    else if (argc >= 4 && argc <= 6 && strcmp(argv[1], "planner") == 0) {
      validWpt = 1;
      waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;

      if (sscanf(argv[2], "%lf", &waypt_message.xyzt[0]) != 1) {
        validWpt = 0;
      }
      if (sscanf(argv[3], "%lf", &waypt_message.xyzt[1]) != 1) {
        validWpt = 0;
      }
      if (argc >= 5) {
        if (sscanf(argv[4], "%lf", &waypt_message.xyzt[2]) != 1) {
          validWpt = 0;
        }
      }
      if (argc >= 6) {
        if (sscanf(argv[5], "%lf", &waypt_message.xyzt[3]) != 1) {
          validWpt = 0;
        }
        waypt_message.xyzt[3] = bot_to_radians(waypt_message.xyzt[3]);
        //sanity check
        if (waypt_message.xyzt[3] < -M_PI || waypt_message.xyzt[3] > M_PI) {
          printf("Error: yaw value must be betw -180 & 180\n");
          validWpt = 0;
        }
      }

      if (validWpt) {
        printf("Sending Planner Waypoint to X: %.3f Y: %.3f\n",
               waypt_message.xyzt[0], waypt_message.xyzt[1]);
        send_thru_planner = true;
        /*
         waypt_message.nonce = random();
         waypt_message.utime = bot_timestamp_now();
         waypt_message.sender = QUAD_WAYPOINT_T_SENDER_WAYPOINT_TOOL;
         //waypt_msg.xyzt_dot[0] = state->current_controller_setpoint->xyzt_dot[0]; //QUAD_WAYPOINT_T_USE_DEFAULT;
         //waypt_msg.xyzt_dot[1] = state->current_controller_setpoint->xyzt_dot[1]; //QUAD_WAYPOINT_T_USE_DEFAULT;
         carmen3d_controller_sanity_check_and_publish(lcm, WAYPOINT_PLANNER_GOAL_CHANNEL, &waypt_message);
         */
      }
    }

    else if (argc >= 3 && argc <= 9) {
      validWpt = 1;
      waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_WAYPT;
      if (sscanf(argv[1], "%lf", &waypt_message.xyzt[0]) != 1)
        validWpt = 0;
      if (sscanf(argv[2], "%lf", &waypt_message.xyzt[1]) != 1)
        validWpt = 0;
      if (argc >= 4 && sscanf(argv[3], "%lf", &waypt_message.xyzt[2]) != 1)
        validWpt = 0;
      if (argc >= 5) {
        if (sscanf(argv[4], "%lf", &waypt_message.xyzt[3]) != 1)
          validWpt = 0;
        else
          waypt_message.xyzt[3] = bot_to_radians(waypt_message.xyzt[3]);
      }
      if (argc >= 6 && sscanf(argv[5], "%lf", &waypt_message.xyzt_dot[0]) != 1)
        validWpt = 0;
      if (argc >= 7 && sscanf(argv[6], "%lf", &waypt_message.xyzt_dot[1]) != 1)
        validWpt = 0;
      if (argc >= 8 && sscanf(argv[7], "%lf", &waypt_message.xyzt_dot[2]) != 1)
        validWpt = 0;
      if (argc >= 9 && sscanf(argv[8], "%lf", &waypt_message.xyzt_dot[3]) != 1)
        validWpt = 0;

      //sanity checks
      if ((waypt_message.xyzt[2] < .1 || waypt_message.xyzt[2] > 3.5)
          && waypt_message.xyzt[2] != QUAD_WAYPOINT_T_USE_PREVIOUS) {
        printf(
            "Error: Height must be between .1 and 3.5m use the land command if you want to land!\n");
        validWpt = 0;
      }
      if ((waypt_message.xyzt[3] < -M_PI || waypt_message.xyzt[3] > M_PI)
          && waypt_message.xyzt[3] != QUAD_WAYPOINT_T_USE_PREVIOUS) {
        printf("Error: yaw value must be betw -180 & 180\n");
        validWpt = 0;
      }

      if ((waypt_message.xyzt_dot[0] <= 0 || waypt_message.xyzt_dot[0] > 5)
          && waypt_message.xyzt_dot[0] != QUAD_WAYPOINT_T_USE_DEFAULT) {
        printf("Error: vx must be greateer than 0 and less than 5 m/s!\n");
        validWpt = 0;
      }
      if ((waypt_message.xyzt_dot[1] <= 0 || waypt_message.xyzt_dot[1] > 5)
          && waypt_message.xyzt_dot[1] != QUAD_WAYPOINT_T_USE_DEFAULT) {
        printf("Error: vy must be greateer than 0 and less than 5 m/s!\n");
        validWpt = 0;
      }
      if ((waypt_message.xyzt_dot[2] <= 0 || waypt_message.xyzt_dot[2] > 1.5)
          && waypt_message.xyzt_dot[2] != QUAD_WAYPOINT_T_USE_DEFAULT) {
        printf("Error: vz must be greateer than 0 and less than 1.5 m/s!\n");
        validWpt = 0;
      }
      if ((waypt_message.xyzt_dot[3] <= 0 || waypt_message.xyzt_dot[3] > 1)
          && waypt_message.xyzt_dot[3] != QUAD_WAYPOINT_T_USE_DEFAULT) {
        printf("Error: vz must be greateer than 0 and less than 1 rad/s!\n");
        validWpt = 0;
      }

      if (validWpt) {
        printf("sending WAYPT: ");
      }

    }

    if (!validWpt) {
      printf("input is invalid. Please enter:\n");
      printf("%s land\n", argv[0]);
      printf("%s takeoff\n", argv[0]);
      printf("%s origin\n", argv[0]);
      printf("%s height <z(m)> <dz(m/s)>\n", argv[0]);
      printf("%s yaw <yaw(deg)>\n", argv[0]);
      printf("%s planner <x> <y> <z> <yaw>\n", argv[0]);
      printf(
          "%s <x(m)> <y(m)> <z(m)> <yaw(deg)> <vx(m/s)> <vy(m/s)> <vz(m/s)> <vyaw(rad/s)>\n",
          argv[0]);
      printf("%s random\n", argv[0]);
      printf("%s random_per <fraction of max velocity>\n", argv[0]);
      printf("%s step <pitch> <roll> <yaw> <thrust> <duration>\n", argv[0]);
      exit(1);
    } else {
      double printYaw;
      if (waypt_message.xyzt[3] == QUAD_WAYPOINT_T_USE_PREVIOUS)
        printYaw = waypt_message.xyzt[3];
      else
        printYaw = bot_to_degrees(waypt_message.xyzt[3]);

      if (send_thru_planner)
        printf("PLANNER: ");

      printf(
          " X=%+4.2f Y=%+4.2f Z=%+4.2f yaw=%+4.2f vX=%+4.2f vY=%+4.2f vZ=%+4.2f type=%d\n",
          waypt_message.xyzt[0], waypt_message.xyzt[1], waypt_message.xyzt[2],
          printYaw, waypt_message.xyzt_dot[0], waypt_message.xyzt_dot[1],
          waypt_message.xyzt_dot[2], waypt_message.waypt_type);

    }
    waypt_message.nonce = random();
    waypt_message.utime = bot_timestamp_now();
    waypt_message.sender = QUAD_WAYPOINT_T_SENDER_WAYPOINT_TOOL;
    if (send_thru_planner) {
      carmen3d_controller_sanity_check_and_publish(
          lcm, WAYPOINT_PLANNER_GOAL_CHANNEL, &waypt_message);
    } else {
      carmen3d_controller_sanity_check_and_publish(lcm,
                                                   WAYPOINT_COMMAND_CHANNEL,
                                                   &waypt_message);
    }

    if (repeat > 0) {
      usleep(repeat * 1e6);
    } else
      break;

  }

  return 0;
}
